#include "SBWander.h"
#include "AIAgent.h"
#include "Timer.h"
#include "DegToRad.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <random>
#include <iostream>




SBWander::SBWander(AIAgent* agent) : Behaviour(agent), m_circleRadius(8), m_circleDistance(8), m_wanderAngle(0), m_angleChange(8.0f)
{
	m_debugDrawColour = Vec3f(0, 0.4f, 1);
}


Vec3f SBWander::Update()
{
	

	
	Vec3f circleCenter = m_pAgent->GetVel();
	circleCenter.Normalise();
	circleCenter = circleCenter * m_circleDistance;

	/*
	Vec3f displacement ( rand(), 0, rand() );
	displacement.Truncate(m_circleRadius);
	//displacement =  displacement * m_circleRadius;

	float len = displacement.Length();
	//float len = m_circleRadius;
	*/
	Vec3f displacement;
	displacement.X = cos(m_wanderAngle) * m_circleRadius;
	displacement.Z = sin(m_wanderAngle) * m_circleRadius;
	

	//srand(m_wanderAngle * 2);
	float random = (float)(rand() % 10) / 10;
	//float random2 = (float)(rand() % 10) / 10;
	//random /= 10;
	
	float wanderAngleChange = ((random * m_angleChange) - (m_angleChange * 0.45f));
	m_wanderAngle += wanderAngleChange * TheTimer::Instance()->GetDT();

	if(m_pAgent->GetID() == 8)
	{
		//std::cout << random << std::endl;
		std::cout << wanderAngleChange << std::endl;
	}

	return displacement;
   //Vec3f pos = m_pAgent->GetPos();
   //
   //if(pos.X > 15 || pos.X < -15)
   //{
	//   m_pAgent->SetPos(Vec3f());
   //}
   //if(pos.Z > 15 || pos.Z < -15)
   //{
	//   m_pAgent->SetPos(Vec3f());
   //}
   //
	//return displacement;

   // Finally calculate and return the wander force
   //Vec3f wanderForce = circleCenter + displacement;
   
   //return wanderForce;
}
 